/*
 * Copyright (c) 2006-2025, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2025-06-26     RT-Thread    first version
 */

#include <rtthread.h>
#include "head.h"

#define DBG_TAG "main"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

rt_thread_t thread_1;
rt_thread_t thread_2;


rt_mutex_t vl53l0_mutex;    //保护vl53l0x_data.RangeMilliMeter变量
rt_mutex_t state_mutex;     //保护car_lock_state变量

uint8_t idle_data[] = {0xAA,0x69,0x64,0x6C,0x65,0xFF};
uint8_t unidle_data[] = {0xAA,0x75,0x6E,0x69,0x64,0x6C,0x65,0xFF};

volatile CAR_LOCK_STATE_T car_lock_state = STATE_RASI;   //地锁状态 初始化为升起（稳定状态）

int main(void)
{
    vl53l0x_init(&vl53l0x_dev);     //vl53l0初始化
    motor_init();
    uart2_init();
    uart3_init();
    buzzer_init();
    /*--------------------VL53L0线程部分------------------------------*/
    thread_1 = rt_thread_create("vl53l0",
                                 vl53l0_thread, RT_NULL,
                                 1024, 20, 50);
    if(thread_1 != RT_NULL)
    {
        rt_thread_startup(thread_1);
        LOG_D("create vl53l0 thread sucess ...\n");
    }
    else
    {
        LOG_E("create vl53l0 thread faild ...\n");
    }

    //创建锁
    vl53l0_mutex = rt_mutex_create("tof_mute", RT_IPC_FLAG_PRIO);
    if(vl53l0_mutex == RT_NULL)
    {
        LOG_E("create vl53l0_mutex faild ...\n");
        return RT_ERROR;
    }
    /*---------------------------------------------------------------*/

    /*--------------------地锁动作线程部分----------------------------*/

    thread_2 = rt_thread_create("car_lock", car_lock_thread, NULL, 512, 21, 10);
    if(thread_2 != RT_NULL)
    {
        rt_thread_startup(thread_2);
        LOG_D("car_lock_thread create sucess ...\n");
    }
    state_mutex = rt_mutex_create("LockSta", RT_IPC_FLAG_PRIO);
    if(state_mutex == RT_NULL)
    {
        LOG_E("create state mutex failed ...\n");
        return RT_ERROR;
    }
    /*-----------------------------------------------------------------*/


    return RT_EOK;
}

//地锁动作线程
void car_lock_thread(void *parameter)
{
    uint16_t distance;  //保存激光测距距离
    uint8_t  position;  //地锁升起位置
    CAR_LOCK_STATE_T current_state;
    static rt_tick_t action_start_time = 0; //开始降下时间
    uint8_t cnt = 0;
    while(1)
    {   //保护临界资源 获取距离
        rt_mutex_take(vl53l0_mutex, RT_WAITING_FOREVER);
        distance = vl53l0x_data.RangeMilliMeter;
        rt_mutex_release(vl53l0_mutex);

        position = debounce_position_pin();   //获取地锁位置
//        position = rt_pin_read(POSITION_PIN);
        current_state = get_car_lock_state();
        set_right_car_flag();
        switch (current_state)
        {
            case STATE_RASI: // 升起（稳定状态）
                // 检查是否需要降下
                if(distance < 900 && position == 0 && get_right_car_flag() == 1)
                {
                    set_car_lock_state(STATE_SHOULD_FALL);
                }
                break;

            case STATE_FALL: // 降下（稳定状态）
                // 检查是否需要升起
                if(distance > 1000 && position == 1)
                {
                    set_car_lock_state(STATE_SHOULD_RAISE);
                }
                break;

            case STATE_SHOULD_FALL: // 应该降下
                // 启动降下动作
                motor_set_pwm(100);
                action_start_time = rt_tick_get();
                set_car_lock_state(STATE_FALLING);
                LOG_I("Starting fall action...");
                break;

            case STATE_SHOULD_RAISE: // 应该升起
                // 启动升起动作
                motor_set_pwm(-95);
                set_car_lock_state(STATE_RAISING);
                LOG_I("Starting raise action...");
                break;

            case STATE_FALLING: // 降下中
                // 检查是否完成降下
                if(rt_tick_get() - action_start_time >= 900)   //降下时间2000ms
                {
                    motor_stop();
                    set_car_lock_state(STATE_FALL);
                    LOG_I("Fall completed");
                }
                break;

            case STATE_RAISING: // 升起中
                // 检查是否完成升起
                if(position == 0)
                {
                    motor_stop();
                    set_car_lock_state(STATE_RASI);
                    LOG_I("Raise completed");
                }
                break;

            default:
                break;
        }
        if(current_state == STATE_RASI)
        {
            rt_device_write(serial_3, 0, idle_data,sizeof(idle_data));
        }
        else if(current_state == STATE_FALL)
        {
            rt_device_write(serial_3, 0, unidle_data,sizeof(unidle_data));
        }

        if(current_state == STATE_RASI && position == 1)
        {
            cnt ++;
            buzzer_on();
            rt_thread_mdelay(500);
            buzzer_off();
            rt_thread_mdelay(500);
            if(cnt >= 10)
            {
                cnt = 0;
                set_car_lock_state(STATE_SHOULD_RAISE);
            }

        }

        if(position == 0)
        {
            led2_on();
        }
        else
        {
            led2_off();
        }
        rt_thread_mdelay(100);
    }
}

//VL53L0线程
void vl53l0_thread(void *parameter)
{
    while(1)
    {
        vl53l0x_general_start(&vl53l0x_dev,Default_Mode);
        rt_thread_mdelay(100);
    }
}

// 状态设置函数
void set_car_lock_state(CAR_LOCK_STATE_T new_state)
{
    rt_mutex_take(state_mutex, RT_WAITING_FOREVER);
    car_lock_state = new_state;
    rt_mutex_release(state_mutex);
}

// 状态获取函数
CAR_LOCK_STATE_T get_car_lock_state(void)
{
    CAR_LOCK_STATE_T current_state;
    rt_mutex_take(state_mutex, RT_WAITING_FOREVER);
    current_state = car_lock_state;
    rt_mutex_release(state_mutex);
    return current_state;
}

static rt_tick_t last_change_time = 0;
static uint8_t last_stable_position = 0;

uint8_t debounce_position_pin(void)
{
    // 1. 读取当前引脚电平
    uint8_t current = rt_pin_read(POSITION_PIN);
    // 2. 检测电平变化
    if (current != last_stable_position)
    {
        // 3. 检查变化是否稳定
        if (rt_tick_get() - last_change_time > 200)
        {
            // 4. 确认稳定变化
            last_stable_position = current;
        }
    }
    else
    {   // 5. 无变化时更新时间戳
        last_change_time = rt_tick_get();
    }
    // 6. 返回消抖后的稳定状态
    return last_stable_position;
}
